﻿#include <Eigen/Dense>
#include <iostream>
using namespace Eigen;

template <typename Derived>
void print_size(const EigenBase<Derived>& b)
{
    std::cout << "size (rows, cols): " << b.size() << " (" << b.rows()
            << ", " << b.cols() << ")" << std::endl;
}

/// Prints a sub-block of the dense expression.
/// Accepts any dense matrix or array expression, but no sparse objects and no special matrix classes such as DiagonalMatrix.
template <typename Derived>
void print_block(const DenseBase<Derived>& b, int x, int y, int r, int c)
{
    std::cout << "block: " << b.block(x,y,r,c) << std::endl;
}

/// Prints the maximum coefficient of the array or array-expression.
template <typename Derived>
void print_max_coeff(const ArrayBase<Derived> &a)
{
    std::cout << "max: " << a.maxCoeff() << std::endl;
}

/// Prints the inverse condition number of the given matrix or matrix-expression.
template <typename Derived>
void print_inv_cond(const MatrixBase<Derived>& a)
{
    const typename JacobiSVD<typename Derived::PlainObject>::SingularValuesType&
     sing_vals = a.jacobiSvd().singularValues();
    std::cout << "inv cond: " << sing_vals(sing_vals.size()-1) / sing_vals(0) << std::endl;
}

/// Calculate the Euclidean distance between two points.
template <typename DerivedA,typename DerivedB>
typename DerivedA::Scalar squaredist(const MatrixBase<DerivedA>& p1,const MatrixBase<DerivedB>& p2)
{
    return (p1 - p2).squaredNorm();
}

// the best way pass reference "const Vector2d &v"
void passByValue(Vector2d v)
{
    std::cout << "v=" << v << std::endl;
}

inline void demoEigenBase()
{
    // 1. Vector
    std::cout << "Vector===" << std::endl;
    Vector3f vf;
    print_size(vf);
    // vf.asDiagonal() returns a 3x3 diagonal matrix pseudo-expression
    print_size(vf.asDiagonal());

    Vector3f u(1, 0, 0);
    Vector3f v(0, 1, 0);
    Vector3f w = u.cross(v); // cross

    std::cout << "UxV=" << w << std::endl;


    // 2. Matrix
    std::cout << "Matrix===" << std::endl;
    Matrix3f A, B;
    A << 1, 2, 3,     // 初始化A，元素也可以是矩阵，先按列堆叠，再按行堆叠。  
     4, 5, 6,       
     7, 8, 9;  
    w[1] = 3;
    B << u, v, w;
    std::cout << "A = \n" << A << std::endl;
    std::cout << "B = \n" << B << std::endl;

    // 3. With 
    // Eigen can map existing memory into Eigen matrices.
    std::cout << "Map ===" << std::endl;
    float array[3];
    Vector3f::Map(array).fill(10);            // create a temporary Map over array and sets entries to 10
    std::cout << "array: " << array[0] << "," << array[1] << "," << array[2] << std::endl;
    int data[4] = {1, 2, 3, 4};
    Matrix2i mat2x2(data);                    // copies data into mat2x2
    std::cout << "mat2x2 = \n" << mat2x2 << std::endl;
    Matrix2i::Map(data) = 2*mat2x2;           // overwrite elements of data with 2*mat2x2
    MatrixXi::Map(data, 2, 2) += mat2x2;      // adds mat2x2 to elements of data (alternative syntax if size is not know at compile time)
    std::cout << "data=" << data[0] << "," << data[1] << "," << data[2] << "," << data[3] << std::endl;
}

int main(int argc, char *argv[])
{
    int option = 1;
    switch (option)
    {
    case 1:
      // 1. EigenBase
      std::cout << "1. EigenBase //==========" << std::endl;
      demoEigenBase();
      break;
    case 2:
        break;
    default:
        break;
    }

    return 0;
}
